#ifndef __motor_c__ruffi__eurobot08_
#define __motor_c__ruffi__eurobot08_

#include <avr/io.h>
#include "delay.h"
#include "dim.c"
#include "pwm.c"
#include <math.h>

/************************************
** propojeni "bridge20" a ATmega16 **
**                                 **
** levy motor:                     **
** IN11 - PB4                      **
** IN12 - PB3 (OC0)                **
**                                 **
** pravy motor:                    **
** IN21 - PD6                      **
** IN22 - PD7 (OC2)                **
************************************/
void init_motors ( void ) {
	// 1 = vystupni
	DDRB |= ((1<<PB0) | (1<<PB5) | (1<<PB6));
	DDRD |= ((1<<PD7) | (1<<PD6) | (1<<PD5) | (1<<PD4) | (1<<PD0) | (1<<PD1));

	//zastavit motory
	PORTB &= ~(1<<PB4);
	PORTD &= ~(1<<PD6);

	init_timer0_pwm();
	init_timer2_pwm();
}

/* on =1 rozsviti ledku
* on=0 zhasne ledku*/
void led ( int on ) {
	switch (on) {
		case 1: PORTB |= 1;
				break;
		case 0: 
		default: PORTB &= ~1;
				break;
	}
}

/*****************
** speed:       **
** 0 - 255      **
**              **
** direction:   **
** 1 - vpred    **
** 0 - vzad     **
**              **
** right/left   **
**  -255 % 255  **
**              **
*****************/

int max_speed(short int sp){
	if ( sp > 255) return (255);
	else return ( sp );
}

/*
* pouzivat funkce right() a left()
* klidne i ty druhe, ale tyhle jsou jednodussi
*/

/*nastavi rychlost a smer otaceni praveho motoru*/
void right2(int speed, int direction) {
	if ( direction == 1 ) {
		PORTD &= ~(1 << PD1);
		set_timer0_ratio(speed);
	} else {
		PORTD |= (1 << PD1);
		set_timer0_ratio(255-speed);
	}
}

void right ( signed short int speed ) {
	if ( speed >= 0 ) right2(max_speed(speed),1);
	else right2(max_speed(-speed),0);
}

/*leveho motoru*/
void left2(int speed, int direction) {
	if ( direction == 0 ) {
		PORTD &= ~(1 << PD6);
		set_timer2_ratio(speed);
	} else {
		PORTD |= (1 << PD6);
		set_timer2_ratio(255-speed); //255-speed
	}
}
void left( signed short int speed ) {
	if ( speed >= 0 ) left2(max_speed(speed),1);
	else left2(max_speed(-speed),0);
}

/*zastavit oba motory*/
void stop( void ) {
	right(0);
	left(0);
}

/*************************
**  ovladani zavory     **
**  na vypousteni micku **
*************************/
void gate_up ( void ) {
	PORTD |= 1<<PD0;
	PORTD |= 1<<PD4;
}

void gate_down ( void ) {
	PORTD |= 1<<PD4;
	PORTD &= ~(1<<PD0);
}

void gate_stop ( void ) {
	PORTD &= ~(1<<PD0 | 1<<PD4);
}

/********************************
** klapka na rozdelovani micku **
** na dobre a spatne           **
********************************/
void flap_up ( void ) {
	PORTD |= 1<<PD5;
}

void flap_down ( void ) {
	PORTD &= ~(1<<PD5);
}

void flap ( int time ) {
	flap_up();
	delay(time);
	flap_down();
}

/*************
**  vytah   **
*************/
void conveyor_up( void ) {
	PORTB |= 1<<PB6;
	PORTB &= ~(1<<PB5);
}

void conveyor_down( void ) {
	PORTB &= ~(1<<PB6);
	PORTB |= 1<<PB5;
}

void conveyor_stop( void ) {
	PORTB &= ~(1<<PB5 | 1<<PB6);
}

#endif

